#include "Sensor.h"

Sensor::Sensor(Drive *z)
{
	sensorDrive = z;
	gyro = new Gyro(1);
	calc = new PIDCalculator();
	pid = new PIDController(0.0625,0.003,0,gyro,calc);
	
	pid->SetInputRange(-360,360);//-360 degrees to 360 degrees
	pid->SetTolerance(0.15);//tolerance is 0.15% of total range
	pid->SetOutputRange(-1.0,1.0);
}

void Sensor::gyroSubroutine(int direction)//0 for left, 1 for right
{
	int flag = 0; //outside tolerance zone
	printf("gyroSubroutine() function called");
	if(direction == 0)
	{	
		pid->SetSetpoint(-12);	//take left fork
		printf("Setpoint = -10");
	}else if (direction == 1 ){
		pid->SetSetpoint(12);	//take right fork
		printf("Setpoint = 10");
	}else{
		pid->SetSetpoint(12);//default to right fork
		printf("Setpoint = default");
	}//end of else
	gyro->Reset();
	pid->Enable();
	printf("OnTarget: %i", pid->OnTarget());
	while(flag == 0)
	{
		leftOutput = calc->getOutput()*-1.0;
		rightOutput = leftOutput * -1.0;
		sensorDrive->myRobot->TankDrive(leftOutput,rightOutput);
		printf("Gyro:  %f	PID Output: %f\n"	, gyro->GetAngle(), calc->getOutput());
		
		if((gyro->GetAngle()<13) && (gyro->GetAngle()>11))
		{
			flag = 1;
		}else if((gyro->GetAngle()>-13) && (gyro->GetAngle()<-11)){
			flag = 1;
		}
		Wait(0.02);
	}
	pid->Disable();
}//end of gyroSubroutine

Sensor::~Sensor()
{
	printf("*****Sensor Deconstructor Called*****");
	delete pid;
	delete gyro;
	delete sensorDrive;
	delete calc;
	
	pid = NULL;
	gyro = NULL;
	sensorDrive = NULL;
	calc = NULL;
}
